/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <mex.h>

#define DEG_TO_RAD 0.01745329251994
#define zeros(X,Y) mxCreateDoubleMatrix(X,Y,mxREAL);
#define sendup(X) mxSetName(X,#X); mexPutArray(X,"base")

/*mex -v -Wall agp_calc.c */
int choldc(double *pR, double *pA, int N);
void cholsl(double *A, double *b, double *x, int N);
int choldcb(double *pR, double *pA, int N, int  p);
void cholslb(double *A, double *b, double *x, int N, int p);
void gproj3D_fun(double *e,double *J,double *ac,double *X);
void motion3Db7(double *e,double *J,double *X,double *w,double t,int j,int i);

int choldcb(double *R,double *A, int N, int p)
{
    int i,j,k,l,lambda;
    double ir;
    
    for (j=0;j<N;j++)
    {
        lambda=j+p;
        if(lambda>N)
            lambda=N;
        lambda=lambda-j;
/*
	for (i=0;i<lambda;i++)
		R[i+j*p]=A[i+j*p];
*/
        k=j-p;
        if(k<0)
            k=0;
        for (;k<j;k++)
        {
            l=k+p;
            if(l>N)
                l=N;
            l=l-j;
            for (i=0;i<l;i++) R[i+j*p]=R[i+j*p]-R[i+(j-k)+k*p]*R[(j-k)+k*p];
        }
        if(R[j*p]<0)
            return 0;
        ir=1./sqrt(R[j*p]);
        for (i=0;i<lambda;i++) R[i+j*p]=R[i+j*p]*ir;
        
    }
    return 1;
}

void cholslb(double *A, double *b, double *x, int N, int p)
{
    int i,k,stop;
    double sum;
    
    for (i=0;i<N;i++)
    { 
        stop=i-p+1;
        if(stop<0)
            stop=0;
        for (sum=b[i],k=i-1;k>=stop;k--) sum -= A[(i-k)+k*p]*x[k];

        if(fabs(A[i*p])<=0.000000000001)
            printf("BOOGIE");
        x[i]=sum/A[i*p]; /*BOOGIE*/
    }
    for (i=N-1;i>=0;i--)
    {
        stop=p;
        if(i+p>N)
             stop=N-i;
        for (sum=x[i],k=1;k<stop;k++) sum -= A[k+i*p]*x[i+k];
        if(fabs(A[i*p])<=0.000000000001)
            printf("BOOGIE");
        x[i]=sum/A[i*p];/*BOOGIE*/
    } 
}
/* calculate error and jacobian for position measurement model*/
void pos_fun(
        double *e,
        double *J,
        double *pos,
        double *X
        )
{
    int D=6;

    /*residuals */
    e[0]=pos[0]-X[4];
    e[1]=pos[1]-X[5];
    e[2]=pos[2]-X[6];
   
    /* Jacobian */
        
    J[0]=0;
    J[1]=0;
    J[2]=0;
    J[3]=0;
    J[4]=1;
    J[5]=0;
    J[6]=0;

         
    J[D]=0;
    J[D+1]=0;
    J[D+2]=0;
    J[D+3]=0;
    J[D+4]=0;
    J[D+5]=1.;
    J[D+6]=0;

      
    J[2*D]=0;
    J[2*D+1]=0;
    J[2*D+2]=0;
    J[2*D+3]=0;
    J[2*D+4]=0;
    J[2*D+5]=0;
    J[2*D+6]=1.;


}
/* calculate error and jacobian for acceleration measurement model with quaternion rotation 3D position, 3D velocity and 3D acceleration
* @param *e [out]  the 3 element error array
* @param *J [out] 3x13 jacobian
* @param *ac [in] 3 element array containing the 3 acceleration measurements
* @param *X [out] 13 element array containing the current state  

*/
void gproj3D_fun(
        double *e,
        double *J,
        double *ac,
        double *X
        )
{
    double R[12],z,x,y,norm,q1t2,q2t2,q3t2,q4t2,q1q2,q1q3,q1q4,q2q3,q2q4,q3q4,a;
    double dRdq0[9],dRdq1[9],dRdq2[9],dRdq3[9],G=9.81,*q;
    double dxdR0,dxdR3,dxdR6,dydR1,dzdR2,dydR4,dzdR5,dydR7,dzdR8;
    int D=13;

    q=X;
    norm=1/(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);/*BOOGIE can be zero*/
    if(norm==0)
        printf("BOOGIE\n");
   
    q1t2=q[0]*q[0];
    q2t2=q[1]*q[1];
    q3t2=q[2]*q[2];
    q4t2=q[3]*q[3];
    q1q2=q[0]*q[1];
    q2q3=q[1]*q[2];
    q3q4=q[2]*q[3];
    q1q4=q[0]*q[3];
    q2q4=q[1]*q[3];
    q1q3=q[0]*q[2];
    /* building rotation matrix */

    R[0]=(q1t2+q2t2-q3t2-q4t2);
    R[1]=2*(q2q3+q1q4);
    R[2]=2*(q2q4-q1q3);
    R[3]=2*(q2q3-q1q4);
    R[4]=(q1t2-q2t2+q3t2-q4t2);
    R[5]=2*(q3q4+q1q2);	
    R[6]=2*(q2q4+q1q3);
    R[7]=2*(q3q4-q1q2);
    R[8]=(q1t2-q2t2-q3t2+q4t2);

    a=2*norm*norm;
   
    dRdq0[0]= q[0]*norm*2-a*q[0]*R[0];
    dRdq0[1]= q[3]*norm*2-a*q[0]*R[1];
    dRdq0[2]=-q[2]*norm*2-a*q[0]*R[2];
    dRdq0[3]=-q[3]*norm*2-a*q[0]*R[3];
    dRdq0[4]= q[0]*norm*2-a*q[0]*R[4];
    dRdq0[5]= q[1]*norm*2-a*q[0]*R[5];
    dRdq0[6]= q[2]*norm*2-a*q[0]*R[6];
    dRdq0[7]=-q[1]*norm*2-a*q[0]*R[7];
    dRdq0[8]= q[0]*norm*2-a*q[0]*R[8];
    
    dRdq1[0]= q[1]*norm*2-a*q[1]*R[0];
    dRdq1[1]= q[2]*norm*2-a*q[1]*R[1];
    dRdq1[2]= q[3]*norm*2-a*q[1]*R[2];
    dRdq1[3]= q[2]*norm*2-a*q[1]*R[3];
    dRdq1[4]=-q[1]*norm*2-a*q[1]*R[4];
    dRdq1[5]= q[0]*norm*2-a*q[1]*R[5];
    dRdq1[6]= q[3]*norm*2-a*q[1]*R[6];
    dRdq1[7]=-q[0]*norm*2-a*q[1]*R[7];
    dRdq1[8]=-q[1]*norm*2-a*q[1]*R[8]; 
     
    dRdq2[0]=-q[2]*norm*2-a*q[2]*R[0];
    dRdq2[1]= q[1]*norm*2-a*q[2]*R[1];
    dRdq2[2]=-q[0]*norm*2-a*q[2]*R[2]; 
    dRdq2[3]= q[1]*norm*2-a*q[2]*R[3];
    dRdq2[4]= q[2]*norm*2-a*q[2]*R[4];
    dRdq2[5]= q[3]*norm*2-a*q[2]*R[5];
    dRdq2[6]= q[0]*norm*2-a*q[2]*R[6];
    dRdq2[7]= q[3]*norm*2-a*q[2]*R[7];
    dRdq2[8]=-q[2]*norm*2-a*q[2]*R[8];
     
    dRdq3[0]=-q[3]*norm*2-a*q[3]*R[0];
    dRdq3[1]= q[0]*norm*2-a*q[3]*R[1];
    dRdq3[2]= q[1]*norm*2-a*q[3]*R[2];
    dRdq3[3]=-q[0]*norm*2-a*q[3]*R[3]; 
    dRdq3[4]=-q[3]*norm*2-a*q[3]*R[4];
    dRdq3[5]= q[2]*norm*2-a*q[3]*R[5];
    dRdq3[6]= q[1]*norm*2-a*q[3]*R[6];
    dRdq3[7]= q[2]*norm*2-a*q[3]*R[7];
    dRdq3[8]= q[3]*norm*2-a*q[3]*R[8];
    
    R[0]*=norm;
    R[1]*=norm;
    R[2]*=norm;
    R[3]*=norm;
    R[4]*=norm;
    R[5]*=norm;
    R[6]*=norm;
    R[7]*=norm;
    R[8]*=norm;
    /* projecting accelerations to imu frame */
    x=R[0]*X[10]+R[3]*X[11]+R[6]*X[12]+R[6]*G;
    y=R[1]*X[10]+R[4]*X[11]+R[7]*X[12]+R[7]*G;
    z=R[2]*X[10]+R[5]*X[11]+R[8]*X[12]+R[8]*G;
    
    /*residuals */
    e[0]=ac[0]-x;
    e[1]=ac[1]-y;
    e[2]=ac[2]-z;

    dxdR0=X[10];
    dxdR3=X[11];
    dxdR6=X[12]+G;
        
    dydR1=X[10];
    dydR4=X[11];
    dydR7=X[12]+G;
    
    dzdR2=X[10];
    dzdR5=X[11];
    dzdR8=X[12]+G;
        
    /* Jacobian */
        
    J[0]=dxdR0*dRdq0[0]+dxdR3*dRdq0[3]+dxdR6*dRdq0[6];
    J[1]=dxdR0*dRdq1[0]+dxdR3*dRdq1[3]+dxdR6*dRdq1[6];
    J[2]=dxdR0*dRdq2[0]+dxdR3*dRdq2[3]+dxdR6*dRdq2[6];
    J[3]=dxdR0*dRdq3[0]+dxdR3*dRdq3[3]+dxdR6*dRdq3[6];
    J[4]=0;
    J[5]=0;
    J[6]=0;
    J[7]=0;
    J[8]=0;
    J[9]=0;
    J[10]=R[0];       
    J[11]=R[3];          
    J[12]=R[6];           
    
    J[D]=dydR1*dRdq0[1]+dydR4*dRdq0[4]+dydR7*dRdq0[7];
    J[D+1]=dydR1*dRdq1[1]+dydR4*dRdq1[4]+dydR7*dRdq1[7];
    J[D+2]=dydR1*dRdq2[1]+dydR4*dRdq2[4]+dydR7*dRdq2[7];
    J[D+3]=dydR1*dRdq3[1]+dydR4*dRdq3[4]+dydR7*dRdq3[7];
    J[D+4]=0;
    J[D+5]=0;
    J[D+6]=0;
    J[D+7]=0;
    J[D+8]=0;
    J[D+9]=0;
    J[D+10]=R[1];         
    J[D+11]=R[4];       
    J[D+12]=R[7];          

    J[2*D]=dzdR2*dRdq0[2]+dzdR5*dRdq0[5]+dzdR8*dRdq0[8];
    J[2*D+1]=dzdR2*dRdq1[2]+dzdR5*dRdq1[5]+dzdR8*dRdq1[8];
    J[2*D+2]=dzdR2*dRdq2[2]+dzdR5*dRdq2[5]+dzdR8*dRdq2[8];
    J[2*D+3]=dzdR2*dRdq3[2]+dzdR5*dRdq3[5]+dzdR8*dRdq3[8];
    J[2*D+4]=0;
    J[2*D+5]=0;
    J[2*D+6]=0;
    J[2*D+7]=0;
    J[2*D+8]=0;
    J[2*D+9]=0;
    J[2*D+10]=R[2];          
    J[2*D+11]=R[5];     
    J[2*D+12]=R[8];
                 
 
}
/* calculate error and jacobian for motion model with quaternion rotation 3D position, 3D velocity and 3D acceleration
* @param *e [out]  the 13 element error array
* @param *J [out] 13x26 jacobian
* @param *X [out] state array 
* @param *w [in] the qyro rotation represented in quaternion 4 element array
* @param t [in] time since previous state instant in seconds 
* @param j [in] previous state instants index in the state array X  
* @param i [in] current state instants index in the state array X  
*/
 
void motion3Db7(
            double *e,
            double *J,
            double *X,
            double *w,
            double t,   
            int j,
            int i
              )
{
    double tq[4];

    
    int D=13,D2=26;
    /*X2=R*X+t*/
/* q */

    tq[0]=X[D*j]*w[0]-X[D*j+1]*w[1]-X[D*j+2]*w[2]-X[D*j+3]*w[3];
    tq[1]=X[D*j]*w[1]+X[D*j+1]*w[0]+X[D*j+2]*w[3]-X[D*j+3]*w[2];
    tq[2]=X[D*j]*w[2]+X[D*j+2]*w[0]-X[D*j+1]*w[3]+X[D*j+3]*w[1];
    tq[3]=X[D*j]*w[3]+X[D*j+3]*w[0]+X[D*j+1]*w[2]-X[D*j+2]*w[1];

    

    e[0]=X[D*i]  -tq[0];
    e[1]=X[D*i+1]-tq[1];
    e[2]=X[D*i+2]-tq[2];
    e[3]=X[D*i+3]-tq[3];
    
/* t */


    e[4]=X[D*i+4]-X[D*j+4]-X[D*j+7]*t;
    e[5]=X[D*i+5]-X[D*j+5]-X[D*j+8]*t;
    e[6]=X[D*i+6]-X[D*j+6]-X[D*j+9]*t;
/* v */
    e[7]=X[D*i+7]-X[D*j+7]-X[D*j+10]*t;
    e[8]=X[D*i+8]-X[D*j+8]-X[D*j+11]*t;
    e[9]=X[D*i+9]-X[D*j+9]-X[D*j+12]*t;
/* a */    
    e[10]=X[D*i+10]-X[D*j+10];
    e[11]=X[D*i+11]-X[D*j+11];
    e[12]=X[D*i+12]-X[D*j+12];
    
/*    e[0]=-e[0];
    e[1]=-e[1];
    e[2]=-e[2];
    e[3]=-e[3];
    e[4]=-e[4];
    e[5]=-e[5];
    e[6]=-e[6];
    e[7]=-e[7];
    e[8]=-e[8];
    e[9]=-e[9];
    e[10]=-e[10];
    e[11]=-e[11];
    e[12]=-e[12];*/
    /*Jacobian*/
    /*j*/
    J[0]=w[0];
    J[1]=-w[1];
    J[2]=-w[2];
    J[3]=-w[3];
    J[4]=0;
    J[5]=0;
    J[6]=0;
    J[7]=0;
    J[8]=0;
    J[9]=0;
    J[10]=0;
    J[11]=0;
    J[12]=0;
    J[13]=-1;
    J[14]=0;
    J[15]=0;
    J[16]=0;
    J[17]=0;
    J[18]=0;
    J[19]=0;
    J[20]=0;
    J[21]=0;
    J[22]=0;
    J[23]=0;
    J[24]=0;
    J[25]=0;
    J[D2]=w[1];
    J[1+D2]=w[0];
    J[2+D2]=w[3];
    J[3+D2]=-w[2];
    J[4+D2]=0;
    J[5+D2]=0;
    J[6+D2]=0;
    J[7+D2]=0;
    J[8+D2]=0;
    J[9+D2]=0;
    J[10]=0;
    J[11]=0;
    J[12]=0;
    J[13+D2]=0;
    J[14+D2]=-1;
    J[15+D2]=0;
    J[16+D2]=0;
    J[17+D2]=0;
    J[18+D2]=0;
    J[19+D2]=0;
    J[20+D2]=0;
    J[21+D2]=0;
    J[22+D2]=0;
    J[23+D2]=0;
    J[24+D2]=0;
    J[25+D2]=0;
    J[2*D2]=w[2];
    J[1+2*D2]=-w[3];
    J[2+2*D2]=w[0];
    J[3+2*D2]=w[1];
    J[4+2*D2]=0;
    J[5+2*D2]=0;
    J[6+2*D2]=0;
    J[7+2*D2]=0;
    J[8+2*D2]=0;
    J[9+2*D2]=0;
    J[10+2*D2]=0;
    J[11+2*D2]=0;
    J[12+2*D2]=0;
    J[13+2*D2]=0;
    J[14+2*D2]=0;
    J[15+2*D2]=-1;
    J[16+2*D2]=0;
    J[17+2*D2]=0;
    J[18+2*D2]=0;
    J[19+2*D2]=0;
    J[20+2*D2]=0;
    J[21+2*D2]=0;
    J[22+2*D2]=0;
    J[23+2*D2]=0;
    J[24+2*D2]=0;
    J[25+2*D2]=0;
    J[3*D2]=w[3];
    J[1+3*D2]=w[2];
    J[2+3*D2]=-w[1];
    J[3+3*D2]=w[0];
    J[4+3*D2]=0;
    J[5+3*D2]=0;
    J[6+3*D2]=0;
    J[7+3*D2]=0;
    J[8+3*D2]=0;
    J[9+3*D2]=0;
    J[10+3*D2]=0;
    J[11+3*D2]=0;
    J[12+3*D2]=0;
    J[13+3*D2]=0;
    J[14+3*D2]=0;
    J[15+3*D2]=0;
    J[16+3*D2]=-1;
    J[17+3*D2]=0;
    J[18+3*D2]=0;
    J[19+3*D2]=0;
    J[20+3*D2]=0;
    J[21+3*D2]=0;
    J[22+3*D2]=0;
    J[23+3*D2]=0;
    J[24+3*D2]=0;
    J[25+3*D2]=0;
    J[4*D2]=0;
    J[1+4*D2]=0;
    J[2+4*D2]=0;
    J[3+4*D2]=0;
    J[4+4*D2]=1.;
    J[5+4*D2]=0;
    J[6+4*D2]=0;
    J[7+4*D2]=t;
    J[8+4*D2]=0;
    J[9+4*D2]=0;
    J[10+4*D2]=0;
    J[11+4*D2]=0;
    J[12+4*D2]=0;
    J[13+4*D2]=0;
    J[14+4*D2]=0;
    J[15+4*D2]=0;
    J[16+4*D2]=0;
    J[17+4*D2]=-1;
    J[18+4*D2]=0;
    J[19+4*D2]=0;
    J[20+4*D2]=0;
    J[21+4*D2]=0;
    J[22+4*D2]=0;
    J[23+4*D2]=0;
    J[24+4*D2]=0;
    J[25+4*D2]=0;
    J[5*D2]=0;
    J[1+5*D2]=0;
    J[2+5*D2]=0;
    J[3+5*D2]=0;
    J[4+5*D2]=0;
    J[5+5*D2]=1.;
    J[6+5*D2]=0;
    J[7+5*D2]=0;
    J[8+5*D2]=t;
    J[9+5*D2]=0;
    J[10+5*D2]=0;
    J[11+5*D2]=0;
    J[12+5*D2]=0;
    J[13+5*D2]=0;
    J[14+5*D2]=0;
    J[15+5*D2]=0;
    J[16+5*D2]=0;
    J[17+5*D2]=0;
    J[18+5*D2]=-1;
    J[19+5*D2]=0;
    J[20+5*D2]=0;
    J[21+5*D2]=0;
    J[22+5*D2]=0;
    J[23+5*D2]=0;
    J[24+5*D2]=0;
    J[25+5*D2]=0;
    J[6*D2]=0;
    J[1+6*D2]=0;
    J[2+6*D2]=0;
    J[3+6*D2]=0;
    J[4+6*D2]=0;
    J[5+6*D2]=0;
    J[6+6*D2]=1.;
    J[7+6*D2]=0;
    J[8+6*D2]=0;
    J[9+6*D2]=t;
    J[10+6*D2]=0;
    J[11+6*D2]=0;
    J[12+6*D2]=0;
    J[13+6*D2]=0;
    J[14+6*D2]=0;
    J[15+6*D2]=0;
    J[16+6*D2]=0;
    J[17+6*D2]=0;
    J[18+6*D2]=0;
    J[19+6*D2]=-1;
    J[20+6*D2]=0;
    J[21+6*D2]=0;
    J[22+6*D2]=0;
    J[23+6*D2]=0;
    J[24+6*D2]=0;
    J[25+6*D2]=0;
    J[7*D2]=0;
    J[1+7*D2]=0;
    J[2+7*D2]=0;
    J[3+7*D2]=0;
    J[4+7*D2]=0;
    J[5+7*D2]=0;
    J[6+7*D2]=0;
    J[7+7*D2]=1;
    J[8+7*D2]=0;
    J[9+7*D2]=0;
    J[10+7*D2]=t;
    J[11+7*D2]=0;
    J[12+7*D2]=0;
    J[13+7*D2]=0;
    J[14+7*D2]=0;
    J[15+7*D2]=0;
    J[16+7*D2]=0;
    J[17+7*D2]=0;
    J[18+7*D2]=0;
    J[19+7*D2]=0;
    J[20+7*D2]=-1;
    J[21+7*D2]=0;
    J[22+7*D2]=0;
    J[23+7*D2]=0;
    J[24+7*D2]=0;
    J[25+7*D2]=0;
    J[8*D2]=0;
    J[1+8*D2]=0;
    J[2+8*D2]=0;
    J[3+8*D2]=0;
    J[4+8*D2]=0;
    J[5+8*D2]=0;
    J[6+8*D2]=0;
    J[7+8*D2]=0;
    J[8+8*D2]=1;
    J[9+8*D2]=0;
    J[10+8*D2]=0;
    J[11+8*D2]=t;
    J[12+8*D2]=0;
    J[13+8*D2]=0;
    J[14+8*D2]=0;
    J[15+8*D2]=0;
    J[16+8*D2]=0;
    J[17+8*D2]=0;
    J[18+8*D2]=0;
    J[19+8*D2]=0;
    J[20+8*D2]=0;
    J[21+8*D2]=-1;
    J[22+8*D2]=0;
    J[23+8*D2]=0;
    J[24+8*D2]=0;
    J[25+8*D2]=0;
    J[9*D2]=0;
    J[1+9*D2]=0;
    J[2+9*D2]=0;
    J[3+9*D2]=0;
    J[4+9*D2]=0;
    J[5+9*D2]=0;
    J[6+9*D2]=0;
    J[7+9*D2]=0;
    J[8+9*D2]=0;
    J[9+9*D2]=1;
    J[10+9*D2]=0;
    J[11+9*D2]=0;
    J[12+9*D2]=t;
    J[13+9*D2]=0;
    J[14+9*D2]=0;
    J[15+9*D2]=0;
    J[16+9*D2]=0;
    J[17+9*D2]=0;
    J[18+9*D2]=0;
    J[19+9*D2]=0;
    J[20+9*D2]=0;
    J[21+9*D2]=0;
    J[22+9*D2]=-1;
    J[23+9*D2]=0;
    J[24+9*D2]=0;
    J[25+9*D2]=0;
    J[10*D2]=0;
    J[1+10*D2]=0;
    J[2+10*D2]=0;
    J[3+10*D2]=0;
    J[4+10*D2]=0;
    J[5+10*D2]=0;
    J[6+10*D2]=0;
    J[7+10*D2]=0;
    J[8+10*D2]=0;
    J[9+10*D2]=0;
    J[10+10*D2]=1;
    J[11+10*D2]=0;
    J[12+10*D2]=0;
    J[13+10*D2]=0;
    J[14+10*D2]=0;
    J[15+10*D2]=0;
    J[16+10*D2]=0;
    J[17+10*D2]=0;
    J[18+10*D2]=0;
    J[19+10*D2]=0;
    J[20+10*D2]=0;
    J[21+10*D2]=0;
    J[22+10*D2]=0;
    J[23+10*D2]=-1;
    J[24+10*D2]=0;
    J[25+10*D2]=0;
    J[11*D2]=0;
    J[1+11*D2]=0;
    J[2+11*D2]=0;
    J[3+11*D2]=0;
    J[4+11*D2]=0;
    J[5+11*D2]=0;
    J[6+11*D2]=0;
    J[7+11*D2]=0;
    J[8+11*D2]=0;
    J[9+11*D2]=0;
    J[10+11*D2]=0;
    J[11+11*D2]=1;
    J[12+11*D2]=0;
    J[13+11*D2]=0;
    J[14+11*D2]=0;
    J[15+11*D2]=0;
    J[16+11*D2]=0;
    J[17+11*D2]=0;
    J[18+11*D2]=0;
    J[19+11*D2]=0;
    J[20+11*D2]=0;
    J[21+11*D2]=0;
    J[22+11*D2]=0;
    J[23+11*D2]=0;
    J[24+11*D2]=-1;
    J[25+11*D2]=0;
    J[12*D2]=0;
    J[1+12*D2]=0;
    J[2+12*D2]=0;
    J[3+12*D2]=0;
    J[4+12*D2]=0;
    J[5+12*D2]=0;
    J[6+12*D2]=0;
    J[7+12*D2]=0;
    J[8+12*D2]=0;
    J[9+12*D2]=0;
    J[10+12*D2]=0;
    J[11+12*D2]=0;
    J[12+12*D2]=1;
    J[13+12*D2]=0;
    J[14+12*D2]=0;
    J[15+12*D2]=0;
    J[16+12*D2]=0;
    J[17+12*D2]=0;
    J[18+12*D2]=0;
    J[19+12*D2]=0;
    J[20+12*D2]=0;
    J[21+12*D2]=0;
    J[22+12*D2]=0;
    J[23+12*D2]=0;
    J[24+12*D2]=0;
    J[25+12*D2]=-1;
}

void gyro_2_guater(
        double *q,
        double *gyro)
{
    double theta;

    theta=sqrt(gyro[0]*gyro[0]+gyro[1]*gyro[1]+gyro[2]*gyro[2]);
    if(fabs(theta)<0.00000000001)
    {
        q[0]=1.;
        q[1]=0.;
        q[2]=0.;
        q[3]=0.;
    }
    else
    {   
        q[0]=cos(theta/2.);
        q[1]=sin(theta/2.)*gyro[0]/theta;
        q[2]=sin(theta/2.)*gyro[1]/theta;
        q[3]=sin(theta/2.)*gyro[2]/theta;
    }
}

void integrate_gyros(double *e,
        double *J,
        double *qua,
        double *gyros)
{
    double tq[4],*nq,q[4],theta,the;

    theta=sqrt(gyros[0]*gyros[0]+gyros[1]*gyros[1]+gyros[2]*gyros[2]);
    if(fabs(theta)<0.00000000001)
    {
        q[0]=1;
        q[1]=0;
        q[2]=0;
        q[3]=0;
    }
    else
    {	
        q[0]=cos(theta/2);
        q[1]=sin(theta/2)*gyros[0]/theta;
        q[2]=sin(theta/2)*gyros[1]/theta;
        q[3]=sin(theta/2)*gyros[2]/theta;
    }
    nq=qua;
    tq[0]=q[0]*nq[0]-q[1]*nq[1]-q[2]*nq[2]-q[3]*nq[3];
    tq[1]=q[0]*nq[1]+q[1]*nq[0]+q[2]*nq[3]-q[3]*nq[2];
    tq[2]=q[0]*nq[2]-q[1]*nq[3]+q[2]*nq[0]+q[3]*nq[1];
    tq[3]=q[0]*nq[3]+q[1]*nq[2]-q[2]*nq[1]+q[3]*nq[0];
    /* correct */
    
    /*    v3[0]=v1[0]*v2[0]-v1[1]*v2[1]-v1[2]*v2[2]-v1[3]*v2[3];
    v3[1]=v1[0]*v2[1]+v1[1]*v2[0]-v1[2]*v2[3]+v1[3]*v2[2];
    v3[2]=v1[0]*v2[2]+v1[1]*v2[3]+v1[2]*v2[0]-v1[3]*v2[1];
    v3[3]=v1[0]*v2[3]-v1[1]*v2[2]+v1[2]*v2[1]+v1[3]*v2[0];
    
    v3[0]=v1[0]*v2[0]-v1[1]*v2[1]-v1[2]*v2[2]-v1[3]*v2[3];
    v3[1]=v1[1]*v2[0]+v1[0]*v2[1]+v1[3]*v2[2]-v1[2]*v2[3];
    v3[2]=v1[2]*v2[0]-v1[3]*v2[1]+v1[0]*v2[2]+v1[1]*v2[3];
    v3[3]=v1[3]*v2[0]+v1[2]*v2[1]-v1[1]*v2[2]+v1[0]*v2[3];
    

	tq[0]=nq[0]*q[0]-nq[1]*q[1]-nq[2]*q[2]-nq[3]*q[3];
        tq[1]=nq[0]*q[1]+nq[1]*q[0]+nq[2]*q[3]-nq[3]*q[2];
        tq[2]=nq[0]*q[2]-nq[1]*q[3]+nq[2]*q[0]+nq[3]*q[1];
        tq[3]=nq[0]*q[3]+nq[1]*q[2]-nq[2]*q[1]+nq[3]*q[0];
	*/

    nq=qua+4;
    the=tq[0]*nq[0]+tq[1]*nq[1]+tq[2]*nq[2]+tq[3]*nq[3];

    J[0]=q[0]*nq[0]+q[1]*nq[1]+q[2]*nq[2]+q[3]*nq[3];
    J[1]=-q[1]*nq[0]+q[0]*nq[1]+q[3]*nq[2]-q[2]*nq[3];
    J[2]=-q[2]*nq[0]-q[3]*nq[1]+q[0]*nq[2]+q[1]*nq[3];
    J[3]=-q[3]*nq[0]+q[2]*nq[1]-q[1]*nq[2]+q[0]*nq[3];
    J[4]=tq[0];
    J[5]=tq[1];
    J[6]=tq[2];
    J[7]=tq[3];
    
    e[0]=1-the;
}

void calc_CC(
        double *CC,
        double *C,
        double *S,
	int bandwith,
        int p,
        int D,
        int base)
{
    int i,j,k; 
    
    for(k=0;k<D;k++)
    {
        for(i=0;i<p;i++)
        {
            for(j=i;j<p;j++)
            {
                CC[j-i+(base+i)*bandwith]+=C[k*p+i]*C[k*p+j]*S[k];
            }
        }
    }
}

void calc_Ce(
        double *Ce,
        double *C,
        double *S,
        double *e,
        int p,
        int D,
        int base)
{
    int i,j;
    
    for(i=0;i<D;i++)
    {
        for(j=0;j<p;j++)
        {
            Ce[base+j]+=C[j+i*p]*e[i]*S[i];
        }
    }
}
/** calculates iteration of levenberg-marquardt optimisation for estimating 6D pose and velocity and acceleration trajectory
*from imu measurements and 3D position measurements
* @param *L [in] Dx26 helper array 
* @param *g [in] Dx1 helper array 
* @param *Xpri [in]  1xD array the initial estimated parameter vector 
* @param *Xpos [in] 1xD array the updated parameter vector  
* @param *indg [in] 1xNg array, , ith element tells from which index ith gyro measurement is
* @param *inda [in] 1xNa array , ith element tells from which index ith acceleration measurement is
* @param *inda [in] 1xNa array , ith element tells from which index ith position measurement is
* @param *qyros [in] 3xNg array of gyro measurements
* @param *acc [in] 3xNa array of acceleration measurements
* @param *pos [in] 3xNp array of position measurements
* @param *dt [in] 2xNg array of time differences
* @param *CC[in] Dx26 helper array  
* @param *Ce [in] Dx1 helper array 
* @param *C [in] helper array for jacobians
* @param *Sg [in] 13x1 array diagonal state prediction covariance
* @param *Sa [in] 3x1 array diagonal acceleration prediction covariance 
* @param *Sp [in] 3x1 array diagonal position prediction covariance  
* @param *D [in]  number of estimated parameters
* @param *Ng [in]  number of gyro measurements
* @param *Na [in]  number of acceleration measurements
* @param *Np [in]  number of pose measurements
* @param *lambda [in]  lambda in levenber-marquardt
* @param *lambda [in]  (Ng+Na+Np)x1 error array
*/
void ag_calc(
        double *L,
        double *g,
        double *Xpos,
        double *Xpri,
        double *indg,
        double *inda,
	double *indp,
        double *gyros,
        double *acc,
	double *pos,
        double *dt,
	double *CC,
        double *Ce,
        double *C,
        double *Sg,
        double *Sa,
	double *Sp,
        int D,
        int Ng,
        int Na,
	int Np,
        double lambda,
        double *er
        )
{
    double e[13],q[4];
    int i,j,k,ret,frame,bandwith=26;
    k=0;
    for(j=0;j<Ng;j++) /* for each gyro measurement */
    {

        frame=(int)indg[j];
        if(frame<(D/13-1))
        {
            gyro_2_guater(q,gyros+3*j);
            motion3Db7(e,C,Xpri,q,dt[j],frame,frame+1);
    /*calculate sparse covariance matrix multiplications */
            calc_Ce(Ce,C,Sg,e,26,13,frame*13);
            calc_CC(CC,C,Sg,bandwith,26,13,frame*13);
            er[k]=0;
	    for(i=0;i<13;i++)
	    {
                er[k]+=Sg[i]*e[i]*e[i];
            }
            k++;
        }
        else
            printf("not enough poses correponding to gyro measurement\n");
    }

    for(j=0;j<Na;j++) /* for each acceleration measurement */
    {
        frame=(int)inda[j];
        if(frame<(D/13))
        {
        /*calculate Jacobian and residual */
            gproj3D_fun(e,C,acc+j*3,Xpri+13*frame);
        /*calculate covariance matrix multiplications */
	    calc_Ce(Ce,C,Sa,e,13,3,frame*13);
	    calc_CC(CC,C,Sa,bandwith,13,3,frame*13);     
        /* measurement error */
            er[k]=Sa[0]*e[0]*e[0]+Sa[1]*e[1]*e[1]+Sa[2]*e[2]*e[2];
	    k++;
        }
        else
            printf("not enough poses correponding to acc measurement %d %d\n",frame,j);
    }

    for(j=0;j<Np;j++) /* for each xyx pose measurement */
    {
        frame=(int)indp[j];
        
        /*calculate Jacobian and residual */
        pos_fun(e,C,pos+j*3,Xpri+13*frame);
        /*calculate covariance matrix multiplications */
        calc_Ce(Ce,C,Sp,e,6,3,frame*13);
        calc_CC(CC,C,Sp,bandwith,6,3,frame*13);     
        /* measurement error */
        er[k]=Sp[0]*e[0]*e[0]+Sp[1]*e[1]*e[1]+Sp[2]*e[2]*e[2];
	k++;
    }

    /* should check if previous move was a success before next */
    do
    {
        for(j=0;j<D*26;j++)
            L[j]=CC[j];
        for(j=0;j<D;j++)
            L[j*26]+=lambda;

        ret=choldcb(L, L, D,26);
        cholslb(L,Ce,g,D,26);
    }
    while(ret<0);

    for(j=0;j<D;j++)Xpos[j]=Xpri[j]+g[j];

}

int choldc(double *R,double *A, int N)
{
    int i,j,k;
    double sum,ir=1.;
    
    for(i=N-1;i>=0;i--)
        for(j=i;j>=0;j--)
            R[i*N+j]=A[i*N+j];

    for (i=0;i<N;i++)
    {
        for (j=i;j<N;j++)
        {
            for (sum=R[i+j*N],k=i-1;k>=0;k--) sum -= R[i+k*N]*R[j+k*N];
            
            if (i == j)
            {
                if (sum <= 0.0) 
                    return 0;
                R[i+i*N]=sqrt(sum);
                ir=1./R[i+i*N];
            }
            else R[j+i*N]=sum*ir;
        }
    }
    return 1;
}

void cholsl(double *A, double *b, double *x, int N)
{
    int i,k;
    double sum;
    
    for (i=0;i<N;i++)
    { 
        for (sum=b[i],k=i-1;k>=0;k--) sum -= A[i+k*N]*x[k];
        x[i]=sum/A[i+i*N];
    }
    for (i=N-1;i>=0;i--)
    {
        for (sum=x[i],k=i+1;k<N;k++) sum -= A[k+i*N]*x[k];
        x[i]=sum/A[i+i*N];
    }
}

/******************************************************/
void mexFunction(
                 int nlhs, mxArray *plhs[],
                 int nrhs, const mxArray *prhs[]
                 )
{
    const mxArray  *mpos,*macc,*mgyros,*mXpri,*mindg,*minda,*mindp,*mSa,*mSg,*mSp,*mdt;
    mxArray *mXpos,*me,*mC,*mCC,*mCe,*mL,*mg;
    double *Xpos,*Xpri,*acc,*gyros,*pos,*indg,*inda,*indp,*e,*C,*CC,*Ce,*L,*g,lambda,*Sa,*Sg,*Sp,q[4],norm,*dt;
    
    int i=0,D,Na,Ng,Np,N;	
    static int	expectRhs = 12;
    static int	expectLhs = 5;
    char	buffer[100];

    /* Check for proper number of arguments */
    if (nrhs != expectRhs) 
    {
        sprintf(buffer, "agp_calc requires %d input arguments. Got %d", expectRhs, nrhs);
        mexErrMsgTxt(buffer);
    }
    else if (nlhs > expectLhs) 
    {
        sprintf(buffer, "agp_calc requires %d outputs arguments. Got %d",expectLhs, nlhs);
        mexErrMsgTxt(buffer);
    }

    i=0;
    mXpri = prhs[i++];
    mgyros = prhs[i++];
    macc = prhs[i++];
    mpos = prhs[i++];
    mdt = prhs[i++];
    mindg = prhs[i++];
    minda = prhs[i++];
    mindp = prhs[i++];
    mSg = prhs[i++];
    mSa = prhs[i++];
    mSp = prhs[i++];
    lambda = mxGetScalar(prhs[i++]);

    D=mxGetNumberOfElements(mXpri);
    Np=mxGetNumberOfElements(mindp);
    Na=mxGetNumberOfElements(minda);
    Ng=mxGetNumberOfElements(mindg);
    N=D/13;
    printf("D: %d %d %d %d\n",D,Ng,Na,Np);
    mXpos=zeros(D,1);
    me=zeros(1,Na+Ng+Np);
    mg=zeros(D,1);
    mCC=zeros(26,D);
    mCe=zeros(D,1);
    mC=zeros(13,26);
    mL=zeros(26,D);

    CC=mxGetPr(mCC);
    
    Ce=mxGetPr(mCe);
    C=mxGetPr(mC);
    Xpos=mxGetPr(mXpos);
    Xpri=mxGetPr(mXpri);
    acc=mxGetPr(macc);
    dt=mxGetPr(mdt);
    pos=mxGetPr(mpos);
    gyros=mxGetPr(mgyros);
    L=mxGetPr(mL);
    g=mxGetPr(mg);    
    indg=mxGetPr(mindg);
    inda=mxGetPr(minda);
    indp=mxGetPr(mindp);
    e=mxGetPr(me);
    Sg=mxGetPr(mSg);
    Sa=mxGetPr(mSa);
    Sp=mxGetPr(mSp);
/*	if(mxGetNumberOfElements(mz)!=2*N*num_frames)
	{
		mexErrMsgTxt("slam_ini2 requires that 2*length(ind(:))*num_frames=length(z(:))");
	}
 */
    for(i=0;i<N;i++)
    {
        q[0]=Xpri[i*13];
        q[1]=Xpri[i*13+1];
        q[2]=Xpri[i*13+2];
        q[3]=Xpri[i*13+3];
        norm=sqrt(q[0]*q[0]+q[1]*q[1]+q[2]*q[2]+q[3]*q[3]);
        if(norm<0.00000000001)
        {
            Xpos[i*13]=1.;
            Xpos[i*13+1]=0.;
            Xpos[i*13+2]=0.;
            Xpos[i*13+3]=0.;
        }
        else
        {	
            Xpos[i*13]=Xpri[i*13]/norm;
            Xpos[i*13+1]=Xpri[i*13+1]/norm;
            Xpos[i*13+2]=Xpri[i*13+2]/norm;
            Xpos[i*13+3]=Xpri[i*13+3]/norm;
        }
	Xpos[i*13+4]=Xpri[i*13+4];
        Xpos[i*13+5]=Xpri[i*13+5];
        Xpos[i*13+6]=Xpri[i*13+6];
        Xpos[i*13+7]=Xpri[i*13+7];
	Xpos[i*13+8]=Xpri[i*13+8];
        Xpos[i*13+9]=Xpri[i*13+9];
        Xpos[i*13+10]=Xpri[i*13+10];
        Xpos[i*13+11]=Xpri[i*13+11];
	Xpos[i*13+12]=Xpri[i*13+12];

    }
    /* this function assumes that gyromeasurement are between consecutive time indeces*/ 
    ag_calc(L,g,Xpos,Xpos,indg,inda,indp,gyros,acc,pos,dt,CC,Ce,C,Sg,Sa,Sp,D,Ng,Na,Np,lambda,e);

    i=0;

    plhs[i++]=mXpos;
    plhs[i++]=mCC;
    plhs[i++]=mCe;
    plhs[i++]=mC;
    plhs[i++]=me;
}
